package upm.etsit.gsi.lejos.mazesolver.behaviours;

import upm.etsit.gsi.lejos.mazesolver.DataExchange;
import lejos.nxt.Motor;
import lejos.robotics.navigation.TachoPilot;
import lejos.robotics.subsumption.Behavior;

/**
 * Comportamiento para girar a la derecha en caso de que no pueda seguir avanzando, o en caso de no poder avanzar ni girar, rota 180 grados.
 * @author jrubia
 */
public class TurnRight implements Behavior{
    
    /**
     * Piloto que controla el movimiento del robot.
     */
    private TachoPilot pilot;

    /**
     * Intercambiador de datos.
     */
    private DataExchange DE;

    /**
     * Mapeador.
     */
    private Mapper mapper;

    /**
     * Constructor con parámetros.
     * @param piloto
     * @param DE
     * @param mapeador
     */
    public TurnRight(TachoPilot pilot, DataExchange DE, Mapper mapper) {
        this.pilot = pilot;
        this.mapper = mapper;
        this.DE = DE;
    }

    /**
     * Metodo para saber cuando tiene que tomar el control, implementado por ser un Behavior.
     * @return True si tiene que tomar el control
     */
    public boolean takeControl() {
        return (this.DE.getDirection() == 2);
    }


    /**
     * Si no tiene impedimentos para avanzar en linea recta, sigue haciéndolo, en caso de que lo encuentre, comprueba si puede girar a la derecha,      * en caso negativo se da la vuelta.
     */
    public void action() {
        if(this.DE.getDistance() > DE.getDIS_SEG()) {
            DE.setDirection(0);
        } else {
            Motor.B.rotateTo(-90);
            if(DE.getDistance() < DE.getDIS_SEG()) {
                Motor.B.rotateTo(0);
                this.pilot.rotate(180);
                this.mapper.rotate(-90);
                this.mapper.rotate(-90);
                DE.setDirection(0);
            }
            else {
                Motor.B.rotateTo(0);
                this.pilot.rotate(-90);
                this.mapper.rotate(-90);
                DE.setDirection(0);
            }

        }
        this.DE.setDirection(0);
    }

    /**
     * Cuando este Behavior deja de tener el control, pone la direction del intercambiador de datos a 0, para que tome el control GoStraight.
     */
    public void suppress() {
        this.DE.setDirection(0);

    }

}
